using System;
using System.Diagnostics;
using System.Collections.Generic;
using Dida.Dbg;

namespace Dida
{
	public class Pawn : Actor
	{
		const int MaxSlideIterations = 5;
		const float DefSlideHorThreshold = .5f;

		Sphere[] spheres;
		Sphere boundingSphere;
		Box boundingBox;

		Vector3D pos = new Vector3D();
		float yaw = 0;

		LinkedList<Sensor> sensors = new LinkedList<Sensor>();

		public Pawn(Sphere[] spheres)
		{
			this.spheres = spheres;
			CalcBSphere();
			CalcBBox();
		}

		public static Pawn CreateStackedSpheresPawn(float radius,
			float height, int nSpheres)
		{
			return new Pawn(GetStackedSpheres(radius,height,nSpheres));
		}

		public static Sphere[] GetStackedSpheres(float radius, float height, int nSpheres)
		{
			Sphere[] spheres = new Sphere[nSpheres];
			Vector3D curCenter = new Vector3D(0, radius, 0);
			float step = (height - radius * 2) / (nSpheres - 1);
			for (int i = 0; i < nSpheres; i++)
			{
				spheres[i] = new Sphere(curCenter, radius);

				curCenter.y += step;
			}
			return spheres;
		}

		private void CalcBSphere()
		{
			boundingSphere = new Sphere();

			// calc the bbox to find the center
			Box bbox = Box.NullBox;
			foreach (Sphere sphere in spheres)
				bbox.Union(sphere.BoundingBox);
			boundingSphere.center = (bbox.min + bbox.max) * .5f;

			foreach (Sphere sphere in spheres)
			{
				float curRad = (sphere.center -
					boundingSphere.center).Len + sphere.radius;
				if (curRad > boundingSphere.radius)
					boundingSphere.radius = curRad;
			}
		}

		private void CalcBBox()
		{
			boundingBox = Box.NullBox;
			foreach(Sphere sphere in spheres)
				boundingBox.Union(sphere.BoundingBox);
		}

		protected override SimpleTransform GetTransform()
		{
			return new SimpleTransform(pos, Quaternion.YawQuaternion(yaw));
		}

		public override Box BoundingBox
		{
			get
			{
				// convert the object space bbox to a world space bbox.
				return new Box(boundingBox.min + pos, boundingBox.max + pos);
			}
		}

		public Vector3D Position
		{
			get { return pos; }
			set
			{
				pos = value;
			}
		}

		public bool Move(Vector3D motion, float startTime, float endTime,
			CollisionResponse collisionResponse)
		{
			WorldCollisionResult res;
			return Move(motion, startTime, endTime, collisionResponse, out res);
		}

		public bool Move(Vector3D motion, float startTime, float endTime,
			CollisionResponse collisionResponse,out WorldCollisionResult lastCollisionRes)
		{
			switch (collisionResponse)
			{
			case CollisionResponse.None:
				{
					pos += motion;
					lastCollisionRes = null;
				}
				return false;

			case CollisionResponse.Stop:
				return MoveStopResponse(motion, startTime, endTime,out lastCollisionRes);
			case CollisionResponse.Slide:
				return MoveSlide(motion, startTime, endTime,out lastCollisionRes);
			case CollisionResponse.SlideHorizontally:
				return MoveSlideHorizontally(motion, startTime, endTime, DefSlideHorThreshold,
					out lastCollisionRes);

			default:
				Debug.Fail("Invalid collision response specified");
				lastCollisionRes = null;
				return false;
			}
		}

		private bool MoveStopResponse(Vector3D motion, float startTime, float endTime,
			out WorldCollisionResult collisionRes)
		{
			if (TestForCollision(motion, startTime, endTime, out collisionRes))
			{
				pos += motion * collisionRes.dist;
				return true;
			}
			else
			{
				pos += motion;
				return false;
			}
		}

		private bool MoveSlide(Vector3D motion, float startTime, float endTime,
			out WorldCollisionResult lastCollisionRes)
		{
			bool collision = false;
			lastCollisionRes = null;

			for (int i = 0; i < MaxSlideIterations; i++)
			{
				WorldCollisionResult collisionRes;
				if (TestForCollision(motion, startTime, endTime, out collisionRes))
				{
					collision = true;
					lastCollisionRes = collisionRes;

					pos += motion * collisionRes.dist;

					float remainder = 1 - collisionRes.dist;
					motion *= remainder;

					float normalOffset = Vector3D.DotProduct(
						motion, collisionRes.normal);
					if(normalOffset < 0)
						normalOffset -= .001f;
					else
						normalOffset += .001f;
					motion -= collisionRes.normal * normalOffset;
				}
				else
				{
					pos += motion;
					break;
				}
			}

			return collision;
		}

		public bool MoveSlideHorizontally(Vector3D motion, float startTime, 
			float endTime,float maxSlideHorThreshold,
			out WorldCollisionResult lastCollisionRes)
		{
			bool collision = false;
			lastCollisionRes = null;

			for (int i = 0; i < MaxSlideIterations; i++)
			{
				WorldCollisionResult collisionRes;
				if (TestForCollision(motion, startTime, endTime, out collisionRes))
				{
					collision = true;
					lastCollisionRes = collisionRes;

					pos += motion * collisionRes.dist;

					float remainder = 1 - collisionRes.dist;
					motion *= remainder;

					if ((float)Math.Abs(collisionRes.normal.y) < maxSlideHorThreshold)
					{
						Vector3D horNormal = collisionRes.normal;
						horNormal.y = 0;
						horNormal.Normalize();

						float normalOffset = Vector3D.DotProduct(
						   motion, horNormal);
						if (normalOffset < 0)
							normalOffset -= .001f;
						else
							normalOffset += .001f;
						motion -= horNormal * normalOffset;
					}
					else
					{
						float normalOffset = Vector3D.DotProduct(
						motion, collisionRes.normal);
						if (normalOffset < 0)
							normalOffset -= .001f;
						else
							normalOffset += .001f;
						motion -= collisionRes.normal * normalOffset;
					}					
				}
				else
				{
					pos += motion;
					break;
				}
			}

			return collision;
		}

		private bool TestForCollision(Vector3D dir, float startTime, float endTime,
			out WorldCollisionResult res)
		{
			res = null;
			for (int i = 0; i < spheres.Length; i++)
			{
				Sphere sphere = new Sphere(spheres[i].center + pos,spheres[i].radius);
				WorldCollisionResult curRes;
				if (World.TestForCollision(sphere, dir, startTime, endTime, out curRes))
				{
					if (res == null || curRes.dist < res.dist)
					{
						res = curRes;
					}
				}
			}

			return res != null;
		}

		/*		public void DoMotion(Vector3D motion, float startTime, float endTime)
				{
					Sphere interestingArea = new Sphere(boundingSphere.center + pos,
						boundingSphere.radius + motion.Len);

					CollisionTree.NodeList interestingNodes = 
						world.StaticCollisionHull.GetNodesInSphere(interestingArea);

					CollisionResult res = null;
					foreach (Sphere sphere in spheres)
					{
						CollisionResult curRes;
						Sphere curSphere = sphere.Clone();
						curSphere.center += pos;
						if (interestingNodes.TestForCollision(curSphere, motion, out curRes))
						{
							if (res == null)
								res = curRes;
							else if (curRes.dist < res.dist)
								res = curRes;
						}
					}

					if (res != null)
						pos += motion * res.dist;
					else
						pos += motion;
				}*/

		/// <summary>
		/// Gets or sets the position of this pawn (in world space).
		/// </summary>
		public Vector3D Pos
		{
			get { return pos; }
			set { pos = value; }
		}

		/// <summary>
		/// Gets or sets the yaw rotation of this pawn.
		/// </summary>
		public float Yaw
		{
			get { return yaw; }
			set { yaw = value; }
		}

		public void AddSensor(Sensor sensor)
		{
			Debug.Assert(!sensors.Contains(sensor), "This pawn already contains the given sensor");
			sensors.AddLast(sensor);
			sensor.SetOwner(this);
		}

		public void RemoveSensor(Sensor sensor)
		{
			Debug.Assert(sensors.Contains(sensor),"This pawn doesn't have the given sensor");
			sensors.Remove(sensor);
			sensor.SetOwner(null);
		}

		/// <summary>
		/// Removes all sensors of the given type.
		/// </summary>
		/// <param name="type">The type of which you want to remove the sensors.</param>
		/// <param name="removeDerived">Specifies whether you also want to remove
		/// sensors of a type derived from the given type.</param>
		public void RemoveSensorsByType(Type type, bool removeDerived)
		{
			if(removeDerived)
			{
				for (LinkedListNode<Sensor> it = sensors.First; it != null; it = sensors.Last)
				{
					if (it.Value.GetType().IsSubclassOf(type))
					{
						sensors.Remove(it);
						it.Value.SetOwner(null);
					}
				}
			}
			else
			{
				for (LinkedListNode<Sensor> it = sensors.First; it != null; it = sensors.Last)
				{
					if(it.Value.GetType() == type)
					{
						sensors.Remove(it);
						it.Value.SetOwner(null);
					}
				}
			}
		}

		internal void ApplyMotionToSensors(Vector3D start, Vector3D dir,float startTime,float endTime)
		{
			foreach (Sensor sensor in sensors)
				sensor.OnMotion(start, dir, startTime, endTime);
		}

		public override void DrawDebug(IDebugDrawTarget target)
		{
			UInt32 colour = 0x00ffff00;
			foreach (Sphere sphere in spheres)
			{
				target.DrawSphere(new Sphere(sphere.center + pos,
					sphere.radius), colour);
			}
		}
	}
}